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I .   INTRODUCTION 

Guided  missiles  can  be  classified  into  four  categories 
depending  on  launch  and  target  position  characteristics.  The 
categories  are  Air-to-Air,  Air-to-Surface,  Surface-to-Air,  and 
Surface-to-Surface  missiles. 

Another  classification  among  missiles  is  the  guidance 
system  of  the  missile.  The  missile  can  be  command  or  homing 
guidance. 

In  the  command  guidance  system  the  missile  and  target  are 
continuously  tracked  and  guided  from  one  or  more  friendly 
vantage  points,  and  the  necessary  path  for  intercept  is 
computed  and  relayed  to  the  missile. 

In  the  homing  guidance  system,  the  missile  has  a  homing 
device  onboard  which  can  detect  the  target  and  gives  the 
necessary  path  directions  for  intercept  to  the  missile.  The 
homing  missile  is  further  subdivided  into  classes  having 
active,  semiactive,  and  passive  guidance  systems.  Active 
detection  is  when  the  missile  illuminates  the  target,  i.e., 
with  a  radar,  and  receives  the  reflected  signals.  Semiactive 
detection  is  when  the  target  is  illuminated  by  a  source  other 
than  the  missile  and  the  missile  receives  the  reflected 
signals.  Passive  detection  is  used  when  the  target  is  the 
source  of  energy,  and  the  missile  detects  signals  that 
propagate  from  the  target. 


Each  of  the  missiles  in  the  above  categories  will  employ 
one  or  more  of  the  three  guidance  laws.  These  laws  are  Pursuit 
Guidance,  Line-of -Sight  Guidance,  and  Proportional  Guidance. 
The  first  portion  of  the  missile  flight  path  may  use  one  of 
the  guidance  laws  but  the  terminal  phase  of  flight  may  be  best 
suited  for  another. 

The  present  work  addresses  the  design  and  evaluation  of  a 
semiactive  Surface-to-Air  missile  using  Proportional 
Navigation  as  the  guidance  law.  A  ground  based  target  tracker 
will  also  developed  with  the  target  deviations  in  position  and 
velocity  relayed  to  the  missile. 

Chapter  II  presents  a  description  and  comparison  of  the 
three  different  guidance  laws.  The  Proportional  Navigation 
guidance  law  will  also  be  developed.  In  Chapter  III  the 
missile  and  target  flight  path  models  will  be  developed  using 
the  concepts  of  Chapter  II  and  computer  simulation  studies 
will  be  performed.  Chapter  IV  consists  of  a  Luenberger 
observer  design,  and  an  evaluation  of  the  estimator,  and  the 
guidance  law  over  a  range  of  conditions  will  be  conducted. 
Chapter  V  consists  of  the  development  of  the  ground  target 
tracker  using  the  theory  of  the  Kalman  Filter  and  again, 
computer  simulations  will  be  included  to  determine  the 
accuracy  of  the  target  tracker. 

All  computer  simulations  are  developed  and  conducted  using 
the  Matrix  Laboratory  (MATLAB)  language. 


II.   MISSILE  GUIDANCE 

A.   GUIDANCE  LAW  SELECTION 

The  selection  of  a  guidance  law  is  a  pre-requisite  for 
determining  the  initial  calculations  for  the  model.  The 
missile  guidance  system  measures  the  error  between  the 
missile's  actual  and  desired  course,  computes  the  corrections 
necessary  to  reduce  the  error  based  on  the  guidance  law 
selected,  and  gives  commands  to  the  autopilot  to  activate  the 
controls  required  to  achieve  acceptable  intercept  of  the 
target.  The  miss  distance  and  the  acceleration  required  by  the 
missile  are  functions  of  the  guidance  law. 

1.  Pursuit  Guidance 

The  pursuit  guidance  law  is  illustrated  in  Figure  1 
and  is  described  as  having  the  missile  velocity  vector 
directed  toward  the  target  at  all  times.  The  missile  is  always 
heading  along  the  line-of-sight  from  the  missile  to  the 
target.  This  guidance  law  is  effective  against  slow  moving 
targets,  but  the  missile  may  lack  sufficient  maneuverability 
against  fast  moving  maneuverable  targets. 

2.  Line-of-sight  Guidance 

Line-of-sight  guidance  is  used  in  a  beam-rider  type 
missile  and  is  illustrated  in  Figure  2.  This  guidance  law 
requires  that  the  missile  remain  on  a  line  joining  the  target 
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Figure  1.  Pursuit  Guidance  Trajectory 

and  the  target  tracker.  The  purpose  of  the  target  tracker  is 
to  maintain  the  antenna  boresight  pointing  at  the  center  of 
the  reflecting  area  of  the  target.  This  guidance  scheme 
normally  requires  a  dedicated  fire  control  system  from  launch 
to  intercept  [Ref.l]. 

3 .  Proportional  Navigation  Guidance 

This  guidance  law  requires  that  the  missile  travel  in 
such  a  way  that  its  own  rate  of  turn  is  proportional  to  the 
rate  of  turn  of  the  line-of-sight  from  the  missile  to  the 
target.  Figure  3  illustrates  the  proportional  guidance  scheme 
in  which  the  rate  of  change  of  the  missile  heading  is  made 
proportional  to  the  rate  of  change  of  the  line-of-sight 
between  the  missile  and  the  target.  The  fixed  or  variable 
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Figure  2.  Line-of-Sight  Trajectory 

multiple  between  the  missile  rate  of  turn  and  the  rate  of  turn 
of  the  line-of-sight  is  called  the  navigation  ratio  (NR) .  The 
proportional  navigation  guidance  law  attempts  to  generate  an 
acceleration  command  perpendicular  to  the  line-of-sight.  One 
way  to  achieve  this  could  be  lateral  acceleration  coupled  with 
angular  or  angular  rate  commands  to  place  the  acceleration 
perpendicular  to  the  line-of-sight.  The  advantage  of  this 
guidance  law  is  in  its  effectiveness  against  maneuvering 
targets.  Since  proportional  navigation  guidance  anticipates 
the  targets  future  position,  it  can  attain  a  higher  degree  of 
responsiveness  over  the  other  two  guidance  laws. 

Figure  4  illustrates  the  proportional  navigation 
scheme  [Ref.2].  If  the  seeker  head  of  the  missile  follows  the 
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Figure  3.  Proportional  Guidance  Trajectory 

AT   =  A-  =  -4-    (<•>  R)    +  a)  R  (2.1) 

target,  the  target  acceleration  perpendicular  to  the  line-of- 
sight  will  equal  the  acceleration  of  the  R  vector,  where 
R    =  missile-target  line-of-sight  vector 
R    =  closing  rate  along  R 
w    =  angular  rate  of  change  of  R 
AT   =  target  acceleration  perpendicular  to  R 
AR   =  tangential  acceleration  of  vector  R 
The  term  (uR)  represents  the  vectorial  acceleration  of 
R  and  the  term  d/dw(uR)  represents  the  rate  of  change  of  the 
tangential  velocity.  A  missile  acceleration,  AM,  equal  to  the 
target  acceleration  K^,    at  this  point  will  make  the  line-of- 
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Figure  4.  Proportional  Navigation  Scheme 

sight  parallel  to  its  original  direction.  Since  the  velocity 
R '  is  along  the  vector  R,  a  missile/target  intercept  is 
assured.  Therefore,  from  Equation  (2.1),  the  executed  missile 
acceleration  commands  should  be 


AM  '   2  (uR)    +(&R) 


(2.2) 


Since  the  direction  of  the  velocity  vector  cannot  be 
directly  controlled,  proportional  navigation  is  achieved  by 
controlling  the  commanded  missile  acceleration  (acorn) . 


acorn  -  VM  y 


(2.3) 


where 


VM  =  the  missile  velocity 
7  =  the  rate  of  change  of  the  velocity  vector 
Implementation  of  proportional  navigation  results  in 
the  following  guidance  law 

acorn   =  NR  VH  6  (2.4) 

where 

a   =  the  rate  of  change  of  the  line-of-sight 
With  this  definition,  the  equation  for  the  rate  of 
change  of  the  velocity  vector  can  be  written  as 

Y  =  NR   d  (2.5) 

B.   PROPORTIONAL  NAVIGATION  KINEMATICS 

From  Figure  5  the  following  equations  of  motion  are 
obtained.  The  three  dimensional  linear  model  will  be  developed 
in  Chapter  III,  but  for  simplicity  and  ease  of  understanding, 
the  fundamental  equations  will  be  first  developed  in  the  x  and 
y  planes.  The  velocity  vector  VM  is  at  an  angle  7M  from  the 
established  reference  line.  From  the  geometry  of  the  problem, 
the  missile  flight  path  angle  can  be  easily  determined. 
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Figure  5.  Intercept  Geometry  for  Proportional  Navigation 


(  V    \ 
yM  =  arctan  — ™ 


(2.6) 


where  V,^  and  V^  are  the  components  of  the  missile  velocity 
vector  in  the  x  and  y  directions,  respectively.  The  target 
flight  path  angle  is  found  from  the  same  geometry  and  is 
expressed  as 


arctan 


'Is) 


(2.7) 


where  V^  and  V^  are  the  target  velocities  in  the  x  and  y 
directions,  respectively. 

The  missile-target  line-of-sight  vector  is  R. 


R   =  [  (  XT  -  XM   )2  ♦  (  YT  -   YH   )2  ]1/2         (2.8) 

where  XT  and  YT  are  the  x  and  y  coordinates  of  the  target  and 
XM  and  YM  are  the  x  and  y  coordinates  of  the  missile. 

The  magnitude  of  the  velocity  vector  for  the  missile  and 
target  (VT)  can  be  expressed  as 


Vir"    I    <   vhx)2  +   (   ^*r>2  11/2 


(2.9) 


VT  =    [    (    V„  )2   +    (    V-  )2  ]1/2  (2.10) 


T  L     v     YTX   '  v     'TY 


The  line-of-sight  angle  (a)  is  defined  as 


o=  arctan 


(2.11) 


and  the  rotation  rate  of  the  line-of-sight   (a)   can  be 
expressed  as 


o  = 


V7sin(yT-a)  -V^iniy^a)  (2.12) 


where  VT  sin(7T  -  a)    and  VM  sin(7M  -  a)    are  the  target  and 
missile  velocity  components  normal  to  the  line-of-sight. 


10 


III.   KINEMATIC  REPRESENTATION  OF  A  SKID-TO-TURN  MISSILE 

A.  INTRODUCTION 

Two  basic  methods  of  controlling  the  attitude  of  a  missile 
to  achieve  the  acceleration  commanded  by  the  guidance  law  are 
skid-to-turn  and  bank-to-turn.  In  the  skid-to-turn  method  the 
roll  angle  is  held  to  a  small  quantity  and  is  usually 
considered  zero  in  initial  calculations.  The  magnitude  and 
orientation  of  the  body  acceleration  vector  is  achieved  by 
permitting  the  missile  to  develop  both  an  angle  of  attack  and 
a  sideslip  angle.  The  presence  of  the  sideslip  imparts  a 
skidding  motion  to  the  missile.  The  bank-to-turn  missile 
generally  will  develop  higher  lift  accelerations  than  the 
skid-to-turn  method,  so  that  the  missile  requires  banking 
maneuvers  to  properly  direct  the  control  vector.  To  achieve 
the  desired  orientation,  the  missile  is  rolled  or  banked  so 
that  the  plane  of  maximum  normal  force  is  oriented  in  the 
desired  direction.  The  present  work  assumes  a  skid-to-turn 
missile  that  is  roll  stabilized. 

B .  ASSUMPTIONS 

A  simplified,  point  mass  representation  of  the  missile 
equations  of  motion  will  be  developed  under  the  following 
assumptions. 
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1.  The  missile  thrust  exactly  cancels  drag. 

2.  The  orientation  of  the  missile  can  be  described  by  the 
Euler  angles  that  represent  the  flight  path  angles  in 
pitch  and  yaw. 

3 .  The  seeker  head  angle  rate  is  a  good  estimate  of  the 
line-of -sight  rate. 

4.  For  small  angles  of  attack  and  sideslip,  the  velocity 
vector  is  assumed  to  be  aligned  with  the  body  center 
line  and  if  the  missile  speed  is  maintained  nearly 
constant,  the  missile  acceleration  in  the  x  direction 
is  fairly  close  to  zero. 

C.   SIMPLIFIED  EQUATIONS  OF  MOTION 

Figure  6  illustrates  the  flight  path  geometry  of  the 
missile  or  target,  with  the  velocity  vector  aligned  with  the 
body  centerline.  From  the  flight  path  geometry,  the  missile 
and  target  velocity  magnitudes  are  expressed  as 

vj,-  [  (tWMiWMiW8  )U2  (3.1} 

for  the  missile,  and 

vT=  [(vyMtW+dW"  ]1/2  (3.2) 

for  the  target. 

The  angles  of  attack  (7_pitch)  and  sideslip  (7_yaw)  are 
defined  as  kinematic  quantities  depending  only  on  velocity 
ratios.  From  Figure  6,  the  missile  and  target  flight  path 
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Figure  6.  Flight  Path  Geometry 


angles  can  be  defined  as 


yM  pitch  »  arctan 


ta 


l/T(V2^V2)) 


(3.3) 


yH  yaw  »  arctan 


(3.4) 


Yr  pitch   »  arctan 


re 


k/r(vw)a+(v^)a) 


(3.5) 
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TY 


Y-  yaw  *  arctan  —^ 


TXt 


(3.6) 


If  the  velocity  vector  is  not  aligned  with  the  body 
center line ,  the  angle  of  attack  is  defined  as 


Ik  pitch  »  arctan 


MZ 


(3.7) 


with  the  sideslip  angle  in  the  yaw  plane  remaining  unchanged. 
Figure  7  illustrates  the  sightline  geometry. 
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\                        /        <YT  -   YM) 
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/                     (Xr  -   XM) 

Figure  7.  Sightline  Geometry 

From  the  sightline  geometry,  the  line-of -sight  angles  can  be 
determined. 
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a  pitch  =  arc tan 


ZT-ZM 


{y/l(XT-XH)2+(YT-YM)2)  ) 


(3.8) 


o  yaw  =  arctan 


(3.9) 


The  range  vector  (R)  can  be  defined  as 


R   =  /TUr-X'J,)2+(lrr-y|,)2«-<Zr-ZJ,)2) 


(3.10) 


The  velocity  components  can  be  found  from  the  Euler  angle 
transformation.  The  derivation  of  the  Euler  angles  can  be 
found  in  Reference  4.  The  x,  y,  and  z  directions  are  taken  as 
the  longitudinal,  lateral,  and  normal  axis  of  the  missile, 
respectively.  The  corresponding  angles  that  represent  the 
angular  displacements  are  <p  (roll)  ,  $  (pitch)  ,  and  V  (yaw)  . 
With  the  missile  assumed  to  be  roll  stabilized,  the  roll  angle 
will  be  taken  as  zero  and  not  included.  The  Euler 
transformation  matrix  is  given  as 


cos  (6)cos  (ijr)  cos  (0)  sin(t|r)  -sin(8) 

-sin(i|r)      cos(i|r)       0 
sin(8)cos(i|r)  sin(8)  sin(\|r)  cos(0) 


(3.11) 


where  I  points  north,  J  east,  and  K  down.  The  transformation 
matrix  represents  the  total  transformation  from  the  inertial 
coordinate  system  to  the  missile  body  axes. 
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From  the  above  matrix,  the  linear  components  of  the 
missiles  velocity  can  be  found  using  the  fight  path  angles  in 
the  yaw  and  pitch  planes. 

vhx  =  Vcos(6)cos(i|r))  (3.12) 

vhy  =  V„(cos(6)sin(i|r))  (3>13) 

Vm   =  Vi,(-sin(e))  (3>14) 

A  skid-to-turn  missile  is  controlled  by  generating  the 
required  acceleration  commands  in  the  pitch  and  yaw  plane. 
From  Reference  2 ,  the  commanded  acceleration  in  the  pitch  and 
yaw  planes  are  defined  as 

Acompi cch   =  VM  pi  tch  yM  pitch  (3.15) 

Acomyav  =  VH  yaw  yH  yaw  (3  .  16) 

The  velocity  components  of  the  missile  in  the  pitch  and  yaw 
planes  are  defined  as 

VM  pi  tch  =  VH  cos(  yM  yaw  -  o  yaw  )  (3.17) 

VHyaw  =  Vi,  cos(  yM  pitch)  (3.18) 

The  autopilot  of  the  missile  will  convert  the  commanded 
accelerations  into  angular  rates  jM   pitch  and  yM   yaw.  These 
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angular  rates  are  passed  to  the  control  surface  servos  and  are 
converted  to  the  necessary  fin  deflections  required  to  steer 
the  missile  to  the  desired  course. 

D.  SYSTEM  SIGNAL  FLOW  GRAPH 

Figure  8  shows  the  total  system  signal  flow  graph  in  the 
y  direction,  where  at  launch  the  missile/target  line-of-sight 
is  taken  as  the  x  axis.  The  outputs  from  the  seeker  is  the 
seeker  head  angle  rate,  which  when  multiplied  by  the 
navigation  ratio,  becomes  the  input  to  the  autopilot.  The 
autopilot  provides  the  signals  required  for  the  missile 
dynamics. 

E.  LATERAL  AUTOPILOT 

As  stated  previously,  the  autopilot  receives  commands  from 
a  guidance  computer  and  processes  these  commands  to  control 
the  deflections  of  the  control  surface.  There  are  three 
principle  requirements  when  designing  an  autopilot.  These  are, 
quick  response,  stability,  and  robustness.  The  autopilot 
should  be  able  to  handle  broad  variations  of  the  aerodynamic 
parameters.  For  example,  the  missile  may  be  required  to 
operate  over  a  broad  range  of  Mach  numbers  and  at  various 
altitudes  which  will  effect  the  normal  force  coefficient, 
which  is  a  function  of  the  Mach  number  and  the  altitude. 

In  this  simulation,  a  simple  lateral  autopilot  will  be 
modeled  as  a  first  order  lag  with  a  time  constant  (t)  of  0.33. 
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Figure  8.  System  Flow  Graph 
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This  has  been  demonstrated  in  past  simulations  to  adequately 
approximate  a  more  detailed  autopilot. 

The  lateral  autopilot  controls  the  motion  of  the  missile 
in  the  pitch  and  yaw  planes.  As  in  the  case  of  this  study,  a 
symmetrical  cruciform  missile,  the  pitch  and  yaw  autopilots 
are  often  identical  so  only  one  will  need  to  be  modeled.  With 
the  assumption  that  the  angle  of  attack  is  very  small,  the 
velocity  of  the  missile  is  aligned  with  the  missile 
centerline.  From  Figure  8,  we  have  the  following  expression 

liH=-K<(li*KNR^  (3.19) 

where  fi  is  the  seeker  head  gimble  angle  rate  and  7M  is  the 
angular  acceleration  of  the  missile  flight  path.  K  is  equal  to 
(  1/t) .  The  transfer  function  of  the  autopilot  is  given  as 

**  (5)  =  «M  (3.20) 

0  (S)     S+K 

F.   SEEKER  HEAD  DEVELOPMENT 

A  homing  head,  when  mounted  in  an  airborne  missile,  is 
called  a  seeker.  The  purpose  of  the  seeker  is  to  detect, 
acquire  and  track  a  target  by  sensing  radiation  or  reflected 
energy  from  the  target.  In  the  present  study,  a  narrow  field- 
of-view  seeker  gimballed  to  the  airframe  will  be  designed.  The 
seeker  maintains  the  target  within  this  narrow  f ield-of-view 
by  rotating  the  platform. 
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If  the  platform  is  inertially  stabilized,  rotation  is  achieved 
by  applying  torques  which  are  proportional  to  the  target's 
displacement.  From  the  centerline  of  the  f ield-of-view,  the 
equation  of  motion  is 

T~Xl  (3.21) 

where  T  is  the  applied  torque,  I  is  the  moment  of  inertia,  and 
0  is  the  angular  acceleration  of  the  seeker  head  angle.  From 
Figure  8  and  equation  (3.21)  the  resulting  equation  of  motion 
is 

P  =  7  =  "^  &  "*i  P  +  *i  °  (3.22) 

where  K,  and  K2  are  functions  of  the  time  constants  of  the 
seeker.  The  transfer  function  of  the  seeker  head  can  then  be 
expressed  as 

P  (S)  .        *1 

o    (S)  s2   +  *a  S   +  jq  (3.23) 

6.   CONTINUOUS  TIME  STATE  EQUATIONS 
1.  Missile  Dynamics 

Given  the  continuous  time  state  equation 

x   (t)  =  A  x   (t)  +  B  u   (t)  (3.24) 

the  missile  or  target  state  equation  is  defined. 
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0    0    1. 

(3.25) 


2.  Autopilot  State  Equation 

The  pitch  and  yaw  autopilots  are  identical,  so  the 
following  state  equation  for  the  autopilot  is  defined  as 


Y  = 


-3  0 
0  -3 


Y  M  pi  tch 
Y  *yaw 


3  0 
0  3 


0  pitch 
0  yaw 


(3.26) 


3.  Seeker  Head  State  Equation 

The  seeker  head  continuous  state  equation  with  a  time 
constant  of  0.1  seconds  in  the  pitch  plane  is  defined  as 


3  = 


0    1 
-100  -20 


P  pitch 
$  pitch 


0 
100 


(3.27) 


The  yaw  autopilot  is  identical  to  the  pitch  autopilot,  so  the 
above  equation  can  be  easily  determined  in  the  yaw  plane. 
4.  Continuous  to  Discrete 

Given  the  continuous  time  state  equations,   the 
discrete  time  equations  are  defined. 
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x  (k  +  1)  =  *  x(k)   +  T  u(ic)  (3.28) 

y  (k  ♦  1)  =  Cx(Jc)  +  D  u(ic)  (3.29) 

The  simulation  study  will  use  the  discrete  time  representation 
of  the  given  continuous  time  equations. 

H.   SIMULATION 

This  section  presents  the  results  of  the  missile/target 
engagement  using  the  proportional  navigation  scheme.  The 
following  assumptions  are  made: 

1.  The  missile  is  limited  to  20  g's  in  either  the  yaw  or 
pitch  plane. 

2.  The  target  is  capable  of  a  5  g  maneuver. 

3.  The  seeker  head  system  is  noise  free.  This  will  form  a 
basis  from  which  the  following  chapters  can  be  referred 
to. 

4 .  The  minimum  difference  between  the  targets  position  and 
the  missiles  position,  will  be  used  as  an  estimate  of 
the  miss  distance  parameter. 

5.  The  origin  is  taken  as  coordinates  (0,0,0)  in  the  x,y,z 
plane. 

1.  Constant  velocity  Target 

The  first  scenario  will  be  a  constant  velocity  target 
with  the  following  initial  conditions. 
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Vmx(O)  =  2000  feet/sec 

XT  (0)  =  20,000  feet 

ZT  (0)  =  50  feet 

V^O)  =  1000  feet/sec 
All  other  initial  conditions  are  zero.  As  shown  in  Figure  9, 
a  successful  intercept  occurred  for  the  constant  velocity 
target.  The  minimum  distance  between  the  target  and  missile 
was  0.7665  feet.  Figure  10  is  a  plot  of  the  missile/target 
trajectories  looking  down  on  the  X-Y  plane.  It  shows  the 
target  starting  at  20,000  feet  on  the  X  axis  with  the  missile 
starting  at  the  origin.  Figure  11  is  a  plot  of  the 
missile/target  trajectories  in  the  Y-Z  plane  with  the  missile 
starting  at  the  origin.  Figure  12  is  a  plot  of  the  missile 
commanded  acceleration  parameter.  The  acceleration  increases 
rapidly  during  the  initial  phase  of  the  engagement  as  force  is 
applied  to  the  missile  and  then  drops  off  until  just  prior  to 
intercept.  Figure  13  shows  the  seeker  head  angle  rate  in  the 
yaw  plane  and  Figure  14  is  a  plot  of  the  seeker  head  angle 
rate  in  the  pitch  plane.  These  parameters  also  increase 
rapidly  during  the  first  few  seconds  as  the  target  is  starting 
out  on  its  flight  profile  and  then  decreases  in  time  as  the 
missile  is  on  course  due  to  the  acceleration  commands. 
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Figure  9.  Range  vs  Time 


MISSILE-TARGET  TRAJECTORY  IN  THE  XY  PLANE 


12000 


10000 


8000 


6000 


2000 


MISSILE 


TARGET 


0.5 


X  AXIS  IN  FEET 


Li 
IIP 


Figure  10.  Trajectory  in  the  X-Y  Plane 
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MISSILE-TARGET  TRAJECTORY  IN  THE  Y  Z  P1ANB 
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Figure  11.  Trajectory  in  the  Y-Z  Plane 


Figure  12.  Commanded  Acceleration 
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Figure  13.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 


Figure  14.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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2.  Constant  Acceleration  Target 

The  second  scenario  will  be  a  constant  acceleration 
target  with  the  following  initial  conditions: 

vmx(°)  =  2000  feet/sec 

XT  (0)  =  20,000  feet 

ZT  (0)  =  50  feet 

VtyCO)  =  500  feet/sec 

Arx(°)  ■  -5.0*32.2  feet/sec~2 

AxyCO)  =  3.5*32.2  feet/secA2 

A^fO)  =  3.5*32.2  feet/secA2 
All  other  initial  conditions  are  zero.  Figure  15  is  a  plot  of 
the  minimum  range  between  the  missile  and  target.  The  minimum 
distance  was  1.13  feet.  This  was  as  expected  due  to  the 
changing  target  trajectory.  Figure  16  is  the  plot  of  the 
missile/target  trajectory  in  the  X-Y  plane  and  Figure  17  is 
the  missile/target  trajectory  in  the  X-Z  plane.  Both  plots 
demonstrate  the  ability  of  the  missile  using  the  proportional 
navigation  guidance  law,  to  effectively  intercept  the  target. 
Figure  18  is  a  plot  of  the  missile  acceleration  vs  time.  As 
expected,  in  contrast  to  the  constant  velocity  simulation,  the 
initial  portion  of  the  engagement  is  characterized  by  a  low 
acceleration  magnitude.  As  the  engagement  progress,  however, 
due  to  the  missile  having  to  pull  more  g's  in  order  to 
compensate  for  the  more  rapid  trajectory  of  the  target,  the 
missile  acceleration  increased  rapidly. 
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Figure  19  is  a  plot  of  the  targets  acceleration  vs  time. 
Figures  20  and  21  are  the  seeker  head  angle  rates  in  the  yaw 
and  pitch  planes,  respectively.  As  with  the  acceleration  of 
the  missile,  similar  characteristics  were  realized  for  these 
parameters  due  to  the  fact  that  the  missile  is  constantly 
changing  its  flight  path  to  compensate  for  the  acceleration  of 
the  target. 
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Figure  15.  Range  vs  Time 
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Figure  18.  Missile  Acceleration  vs  Time 


Figure  19.  Target  Acceleration  vs  Time 
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Figure  16.  Trajectory  in  the  X-Y  Plane 
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Figure  17.  Trajectory  in  the  X-Z  Plane 
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Figure  20.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 
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IV.   LUBNBERGER  OBSERVER 

A.  INTRODUCTION 

The  purpose  of  a  Luenberger  observer  for  the  proportional 
navigation  model  developed  in  Chapter  III,  is  to  estimate  the 
variables  in  the  missile/target  engagement  that  might  not  be 
measured  directly  or  to  reduce  noise  on  the  states  that  are 
measured  by  the  missile.  It  is  not  practical  to  design  a 
missile  to  determine  all  inputs.  Such  inputs  could  be  evasive 
maneuvers  of  the  target,  initial  conditions,  noise  level,  or 
the  jamming  techniques  employed.  The  use  of  a  Luenberger 
observer  is  beneficial  in  these  situations. 

B.  OBSERVER  DESIGN 

The  seeker  head  angle  rate  is  assumed  to  be  a  good 
approximation  of  the  line-of -sight  rate.  The  observer  will  be 
designed  to  estimate  the  states  of  the  seeker  head,  since  the 
actual  line-of -sight  rate  of  the  seeker  head  can  be 
contaminated  by  noise  or  jamming.  The  following  equations  are 
described  in  Reference  4.  Given  a  system 

x(k   ♦  1)  =  Qx(k)   +  Au(JO  (4.1) 

and  the  output  equation 

y(k)    =  C  x(k)    +  D  u(k)  (4.2) 

The  observer  will  approximate  x(k)  with  inputs  y(k)  and  u(k) . 
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The  observer  equation  is  given  as 

*(Jc  +  1)  =  F  x(k)    +  G  y(k)    +  H  u(k)  (4.3) 

with  xhat(k)  being  the  estimate  of  the  original  system.  The 
error  between  the  state  x(k)  and  the  estimated  state  can  be 
defined  as 

e(k  *  1)  =  x(k  +  1)  -  x(k  +  1)  (4.4) 

e(k  *  1)  -  *x(Jc)  +  Au(Jc)  -F*(W  -GyU)  -ffu(Jc)      (4.5) 
which  results  in  the  error  equation  of  the  form 

e(k   +  1)  =  (*-GOx(ic)  -F^(ir)  +  (A-GD-H)  u(Jc)    (4.6) 
If  we  select  F  as 

F   =  *-GC  (4.7) 

Equation  (4.4)  becomes  a  state  equation  of  the  following  form 

e(k  +  1)  =  (•-GOe(lc)  +  (A-GD-ff)  u(ic)       (4.8) 

By  selecting  H  as 

H  =  A-GD  (4.9) 

Equation  (4.6),  with  input  u(k) ,  results  in  a  state  equation 
without  input,  of  the  following  form 

e(k  +  1)  =  (*-GOe(ic)  (4.10) 

If  the  original  system  is  observable,  by  selecting  the 

observer  gain  G,  we  can  design  the  observer  such  that  the 

error  approaches  zero  at  a  rate  that  is  desired  based  on  the 
design  characteristics. 
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The  unforced  system 


x(k  +  I)    =  $  x 


with  the  observation  equation 


y(k)    =  C  x(k) 


(4.11) 


(4.12) 


is  observable  if  and  only  if  the  rank  (N)  of  the  observability 
test  matrix 


N  =  [C   */C/ (*)Jc"1C/] 


(4.13) 


is  equal  to  k,  the  order  of  the  system  [Ref.5]. 

The  observer  design  begins  by  converting  the  state 
equation  for  the  seeker  head,  Equation  (3.27),  from  continuous 
time  to  discrete  time  representation.  With  a  sampling  time  of 
0.05  and  an  unknown  disturbance  w(k)  the  equation  is  given  as 


(HJc-l) 


0.9098  0.0303 
-3.0327  0.3033 


♦ 

0.0902 
3.0327, 

a  (it)  * 

0.0013 
0.0513 

w{k) 


(4.14) 


with  the  output  equation  measuring  the  seeker  head  angle  rate 
given  as 


y(k)    =  [0  1] 


P  (ic) 


(4.15) 


The  system  is  observable,  so  the  observer  gain  can  be 
determined.  It  is  usually  desirable  to  select  the  eigenvalues 
of  the  observer  so  that  the  error  between  the  state  and 
estimated  state  will  rapidly  become  small  and  will  not  be 
highly  responsive  to  noise.  Because  the  system  is  observable, 
the  eigenvalues  of  the  observer  can  be  arbitrarily  positioned. 
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If  the  observer  gain  is  high,  the  observer  will  quickly  drive 
the  initial  condition  error  to  zero  but  will  make  the  observer 
more  susceptible  to  noise.  By  choosing  a  low  observer  gain, 
the  observer  response  will  be  slower  but  the  observer  will  be 
less  affected  by  noise  or  jamming.  Previous  studies  have  shown 
that  a  good  compromise  between  the  two  would  be  to  choose 
continuous  time  poles  of  p,  and  p2  to  be  between  the  values  of 
8.0  to  12.0.  Continuous  time  poles  of  10.5  were  chosen  for  the 
observer  which  results  in  the  discrete  time  poles  given  as 


Poi  =Po2  =  e<-10-5-0-05'  =  0.5916 
The  observer  gain  matrix  is  then  given  as 


(4.16) 


G  = 


-0.0031 
0.0299 


(4.17) 


The  F  matrix  is  equal  to  (*  -  GC)  and  is  given  as 


F  = 


0.9098  0.0303 
-3.0327  0.3033 


-0.0031 
0.0299 


[0  1] 


(4.18) 


The  H  matrix  is  equal  to  (A  -  GD)  and  with  D  equal  to  zero  the 
H  matrix  reduces  to  A.  The  observer  equation  for  the  seeker 
head  is  given  as 


[J  <*♦!)] 


0.9098 
-3.0327 


0.0297U 
0.3094P<JC)' 


-0.0031 
0.0299 


yik) 


0.0902 
3.0327 


•  (*) 


(4.19) 
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C.   SIMULATION  RESULTS 

The  two  previous  scenarios  given  in  Chapter  III  were  used 
to  test  the  observer  design.  Random  noise  was  injected  into 
the  seeker  head  to  determine  if  the  Luenberger  Observer  would 
reduce  the  noise  of  the  states  that  were  directly  measured  by 
the  seeker  head  and  generate  the  required  signals  necessary 
for  the  autopilot. 

1.   Constant  Velocity  Target 

The  same  initial  conditions  presented  in  Chapter  III 
for  the  constant  velocity  engagement  will  be  used  for 
comparison  in  this  section.  Figures  22  through  27  are  plots  of 
the  constant  velocity  engagement  without  the  Luenberger 
Observer  but  with  a  random  disturbance.  Figures  22  and  2  3  are 
the  plots  of  the  trajectories  in  the  X-Y  and  X-Z  planes, 
respectively.  The  miss  distance  was  15.6  feet.  The  trajectory 
in  the  Y-Z  plane  depicts  how  the  noise  input  to  the  seeker 
head  adversely  effects  the  missiles  trajectory.  Figures  24  and 
2  5  are  the  seeker  head  angle  rates  in  the  yaw  and  pitch  plane, 
respectively  and  Figure  26  is  the  plot  of  the  commanded 
acceleration  as  a  result  of  the  seeker  head  angle  rates  being 
subjected  to  noise.  Figure  27  is  the  plot  of  the  random  noise 
generated  by  the  program. 
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MJSSILETARGET  TRAJECTORY  IN  THE  X-Y  PLANB 
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Figure  22.  Trajectory  in  the  X-Y  Plane  With  Noise 
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Figure  23.  Trajectory  in  the  Y-Z  Plane  with  Noise 
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Figure  24.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 


Figure  25.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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Figure  26.  Commanded  Acceleration  with  Noise 
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Figure  27.  Random  Noise  Generated  vs  Time 
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The  following  plots  depict  the  results  with  the 
observer  included  in  the  simulation  with  the  same  initial 
conditions.  Figures  28  and  29  are  the  trajectories  in  the  X-Y 
and  Y-Z  planes,  respectively.  In  contrast  with  the  Y-Z  plane 
plot  without  the  observer,  the  missiles  trajectory  is  less 
effected  by  the  random  disturbance.  A  miss  distance  of  1.56 
feet  occurred.  Figures  3  0  and  31  are  the  plots  of  the  seeker 
head  angle  rates  in  the  yaw  and  pitch  plane,  respectively. 
When  compared  to  the  angle  rate  plots  of  Chapter  III,  it  is 
clear  that  the  observer  has  reduced  the  effect  of  noise  to  the 
point  that  the  plots  are  very  similar.  Figures  32  and  33  are 
the  plots  of  the  commanded  acceleration  and  the  random  noise 
generated,  respectfully.  The  commanded  acceleration  is  also 
less  effected  by  the  random  disturbance  when  the  observer  is 
made  part  of  the  missile. 
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MISSILE  TARGET  TRAJECTORY  IN  THE  X-Y  PLANE 
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Figure  28.  Trajectory  in  the  X-Y  Plane  with  Observer 
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Figure  29.  Trajectory  in  the  Y-Z  Plane  with  Observer 
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Figure  30.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 


Figure  31.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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Figure  32.  Commanded  Acceleration  with  Observer 


Figure  33.  Random  Noise  Generated  vs  Time 
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2.   Constant  Acceleration  Target 

The  results  for  the  constant  acceleration  engagement 
were  as  expected,  with  the  observer  reducing  the  effects  of 
noise  so  that  intercept  occurred.  Figures  34  through  39  are 
plots  of  the  constant  acceleration  model  without  the  observer. 
Figures  34  and  35  are  plots  of  the  trajectories  in  the  X-Y  and 
X-Z  planes,  respectively.  These  plots  do  not  show  the 
variations  in  the  trajectories  due  to  the  magnitudes  of  the 
scales.  Figures  36  and  37  are  plots  of  the  seeker  head  angle 
rates  in  the  yaw  and  pitch  planes,  respectively.  When  compared 
to  the  plots  in  Chapter  III,  it  is  obvious  that  the  missiles 
performance  is  degraded.  A  miss  distance  of  26.5  feet 
occurred.  Figures  38  and  39  are  plots  of  the  commanded 
acceleration  and  the  random  noise  generated. 
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Figure  34.  Trajectory  in  the  X-Y  Plane  with  Noise 
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Figure  35.  Trajectory  in  the  X-Z  Plane  with  Noise 
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Figure  36.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 
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Figure  37.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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Figure  38.  Commanded  Acceleration  with  Noise 
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Figure  39.  Random  Noise  Generated  vs  Time 
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The  following  plots  depict  the  constant  acceleration 
engagement  with  the  Luenberger  Observer.  The  observer  reduced 
the  noise  so  that  intercept  occurred  with  a  miss  distance  of 
4.5  feet.  The  plots  in  the  X-Y  and  X-Z  planes  are  not  included 
due  to  the  magnitude  of  the  scales.  Figures  4  0  and  41  are  the 
plots  of  the  seeker  head  angle  rates  in  the  yaw  and  pitch 
planes,  respectfully.  When  compared  with  the  constant 
acceleration  model  in  Chapter  III  it  is  obvious  that  the 
observer  is  beneficial  in  reducing  the  random  disturbance 
added  to  the  seeker  head.  Figure  42  is  the  commanded 
acceleration  of  the  missile. 

In  the  present  day  environment,  with  fast  maneuvering 
targets,  the  simulations  in  this  chapter  demonstrate  the  need 
for  the  observer  concept  to  be  included  in  the  missile  design 
and  also  in  computer  simulation  studies. 


48 


rata  ux*  nt  uou  in  n  to 

1.  N 
1  IT 

0  it 

o.of 
3      I.M 

a 

0.0] 

0.0] 

i                    i 

j 

o L- 

0                   «                   1                   3 

1                    t                    I                    t                    7                    1 

•                  it 

Figure  40.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 


Figure  41.  Seeker  head  Angle  Rate  in  the  Pitch  Plane 
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Figure  42.  Commanded  Acceleration  with  Observer 


The  computer  program  used  to  generate  the  plots  in  this 
chapter  is  included  in  the  appendix,  of  which  a  portion  is  the 
work  completed  previously  by  LT  F.  Lukenbill  in  our  missile 
and  avionics  class. 
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V.   GROUND  TARGET  OBSERVER 

A.   INTRODUCTION 

Air  defense  is  becoming  significantly  more  complex  due  to 
the  increased  speed  and  manueverability  of  hostile  aircraft 
and  the  enemy's  use  of  electronic  countermeasures  (ECM) . 
Attacking  aircraft  will  employ  jamming  as  well  as  other  forms 
of  ECM  to  degrade  the  tracking  performance  of  surface-to-air 
missiles.  Therefore,  many  modern  surface-to-air  missile 
systems  have  target  observers  that  track  incoming  aircraft 
from  the  ground  and  uplink  the  deviations  in  target  position 
and  velocity  to  the  missile. 

In  this  chapter,  a  simplified  Kalman  Tracking  Filter  is 
developed.  The  tracker  consists  of  velocity  and  position 
estimation.  Once  the  missile  is  launched,  target  deviations 
are  computed  by  subtracting  the  target  observer  estimates  from 
the  current  radar  estimates.  At  specified  intervals  in  time, 
the  errors  in  position  and  velocity  are  uplinked  to  the 
missile.  After  reception,  the  missile  converts  the  errors  into 
the  missile  body  reference  frame  via  the  attitude  Euler  angles 
and  then  into  the  line-of-sight  frame  using  the  seeker  head 
angle  transformation.  The  estimated  angular  rate  deviations 
are  then  added  to  the  missile's  current  estimate  and  the 
appropriate  acceleration  commands  are  generated. 
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B.   KALMAM  FILTER 

The  purpose  of  the  Kalman  Filter  is  to  keep  track  of  the 
states  of  a  system  given  a  sequence  of  noisy  measurements. 
This  is  accomplished  by  recursively  updating  an  estimate  of 
the  state  by  processing  a  sequence  of  noisy  observations  so 
that  the  effect  of  the  measurement  errors  are  reduced  as  much 
as  possible. 

First,  previous  estimates  of  the  state  x,  and  the 
covariance  matrix  P,  are  extrapolated  one  step  ahead  based  on 
the  assumed  systems  dynamics.  These  values  are  used  to  compute 
a  set  of  optimum  weights  or  Kalman  Gains.  The  gains  are 
applied  to  the  prediction  and  to  a  new  observation  which 
provides  an  updated  estimate  of  the  state  and  the  covariance 
matrix. 

It  is  assumed  that  the  target  tracker  has  no  a  priori 
information  on  the  target's  position  or  velocity.  Also,  the 
error  covariance  matrix  will  be  set  sufficiently  large  in 
order  that  the  filter  gain  will  be  able  to  make  the  estimate 
converge  to  the  true  value  and  make  the  simulation  independent 
of  any  initial  estimate  states.  The  random  forcing  function  Wk 
is  zero  mean  with  covariance  Qk  and  the  measurement  noise  Vk  is 
zero  mean  with  covariance  R*.  The  Kalman  Filter  algorithm  is 
given  in  Equations  (5.1)  through  (5.5)  in  the  order  that  the 
filter  is  implemented  in  the  computer  program.  The  algorithm 
is  given  as 
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T    j.      D\    -1 


Gk   "   P(k\k-\)    H      W  P(k\k-l)    a      *   RS> 


(5.1) 


&{k\k)    ~  ^(Jc|Jc-l)    +  Gk    (Yk   "  H  *(Jt|Jc-l) ) 


(5.2) 


P(Jc|Jr)    =     (X   -    G^  H)     P(jt|Jc.1) 


(5.3) 


■*(Jc*l|Jc)     =    *    * 


(Jr|Jr) 


(5.4) 


*W|«    =    *    P(*|Jc)    *T  +    0 


(5.5) 


From  Reference  2,  the  following  covariance  matrices  are 
defined.  The  purpose  of  the  random  forcing  function  covariance 
matrix  Q,  is  to  account  for  model  inaccuracies  or  for  a  target 
that  has  maneuvered.  The  covariance  matrix  Q  is  given  as 


0  = 


»i  o   o 

0    oj    0 

« 

0     0    o2, 

1602       0  0 

0       1602       0 
0  0       1602 


(5.6) 


Since  the  actual  range  of  the  target  is  unknown,  an  estimate 
of  the  range  is  used  in  determining  R*.  The  covariance  of 
measurement  error  matrix  will  then  be  based  on  a  three  degree 
field  of  view  radar,  observing  the  target  approaching  an  area 
20,000  feet  from  the  ground  based  tracker. 
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Rk  a 


5002  0  0 
0  1252  0 
0    0   12. 52 


(5.7) 


The  measurement  equation  yk  is  given  as 


y(k)    =  H  x(k)    +  v(Jc) 


(5.8) 


where  H  is  the  measurement  matrix,  a  linear  function  of  the 
state  vector,  and  u(k)  the  measurement  noise. 

C.   UPLINK 

The  uplink  from  the  ground  target  observer  consists  of 
deviations  in  target  position  and  velocity.  The  deviations  are 
calculated  by  determining  the  difference  between  the  actual 
noisy  measurement  and  the  predicted  state  estimate. 


y(k)    -  HJtlklk.x 


\k-l) 


(5.9) 


Upon  reception,  the  missile  transforms  the  deviations  into  the 
missile  body  frame. 


V, 


BX 


BY 


BZ 


cos  (6)  cos  (\|r)  cos  (6)  sin(\|r)  -sin(0) 

-sin(\|r)      cos(i|r)       0 
sin(8)  cos  (i|r)  sin(9)  sin(i|0  cos  (6). 


v. 


(5.10) 


where  Vx,  VY,  and  Vz  are  the  velocity  deviations  in  the  x,  y, 
and  z  directions,  respecttively,  and  Vbx,  Vby,  and  VBZ  are  the 
transformed  velocities  in  the  missile  reference  frame.  The 
range  deviations  are  calculated  the  same  and  are  defined! 
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Lax 


'BY 


R 


BZ. 


cos  (6)  cos  (i|r)  cos  (8)  sin(ijr)  -sin(9) 

-sin(\|r)       cos(i|f)       0 
sin(8)  cos  (\|0  sin(6)  sin  (\|r)  cos(9) 


R, 


(5.11) 


where  Rx,  RY/  and  Rz  are  the  range  deviations  and  RBX/  RBY  and 
RBZ  are  the  transformed  range  deviations  in  the  x,  y,  and  z 
directions,  respectively.  The  transformation  to  the  line-of- 
sight  reference  frame  is  accomplished  by  using  the  seeker  head 
angle  transformation  and  is  given  as 


sx 


SY 


3Z. 


cos  (op)  cos  (or)  cos  (ap)  sin(ar)  -sin(<jp) 


-sin(oy) 


cos  (or) 


sin(Op)  cos  (or)  sin(Op)  sin(oy)  cos(op)J  VK 


ax 


(5.12) 


where  ap  and  aY  are  the  line-of-sight  angles  in  the  pitch  and 
yaw  plane,  respectively.  Vjx,  VSY,  and  Vsz  are  the  transformed 
velocities.  The  range  is  transformed  using  the  same 
transformation  matrix  and  is  given  as 


^sx 


'SY 


sz. 


cos  (o p)  cos  (o r)    cos  (Op)  sin (or)  -sin(ap) 

-sin(or)       cos(oy)       0 
sin(Op)  cos  (ar)  sin (op)  sin (ar)  cos(op) 


^BX 


v*r 


az. 


(5.13) 


where  R^,  RgY,  and  RgZ  are  the  transformed  ranges  in  the  x,  y, 
and  z  directions,  respectively.  The  final  process  in  the 
uplink  transformation  is  to  generate  the  angular  rate 
deviations.  These  are  calculated  by  dividing  the  velocity 
deviations  by  the  range  and  the  magnitude  of  the  range 
deviations.  The  line-of-sight  rate  in  the  yaw  plane  is  given 
as  Equation  (5.14). 
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'y-TTir,  (5-14) 


where   RD    is   given   as 

RD  =  vT  Rsx  *  Rsy  *  Rsz  )  (5.15) 

and  the   line-of-sight  rate   in  the  pitch  plane  defined  as 

6  ,  Vsz 


'itch  '     R  +  RD  (5.16) 

D.   SIMULATION  OF  THE  TARGET  OBSERVER 

The  constant  velocity  target  in  a  noisy  environment  will 
be  the  first  test  case  for  the  ground  target  observer.  Figures 
43  through  51  summarize  the  extensive  estimator  performance 
calculations  for  the  constant  velocity  scenario.  Figures  4  3 
through  48  are  the  histories  of  the  six  elements  of  the  state 
vector.  An  inspection  of  these  figures  indicates  that  the 
estimator  tracks  the  system  in  position  and  velocity  quite 
well.  Figures  49  through  51  are  examplese  of  the  Kalman  Filter 
Gains  vs  time.  Figures  52  through  54  are  the  response  of  the 
missile  to  the  uplink  commands.  The  deviations  in  position  and 
velocity  were  uplinked  at  one  second  intervals.  The  miss 
distance  parameter  was  observed  to  be  0.44  feet. 
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Figure  43.  Target  Position  in  the  X  Direction 
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Figure  44.  Target  Velocity  in  the  X  Direction 
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Figure  45.  Target  Position  in  the  Y  Direction 
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Figure  46.  Target  Velocity  in  the  Y  Direction 
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Figure  47.  Target  Position  in  the  Z  Direction 
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Figure  48.  Target  Velocity  in  the  Z  Direction 
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Figure  49.  Kalman  Gains  in  the  X  Direction 
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Figure  50.  Kalman  Gains  in  the  Y  Direction 
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Figure  51.  Kalman  Gains  in  the  Z  Direction 


Figure  52 .  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 
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Figure  53.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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Figure  54.  Acceleration  Commanded 
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For  the  case  of  a  maneuvering  target,  the  constant 
acceleration  model  was  used  to  determine  filter  performance. 
As  expected,  filter  performance  was  marginal.  Filter 
convergence  to  an  accuracy  attained  in  the  constant  velocity 
target  did  not  occur.  Figures  55  through  63  summarize  the 
estimator  performance  for  the  constant  acceleration  scenario. 
Figures  55  through  60  are  the  six  elements  of  the  state 
vector.  As  shown  in  these  plots,  the  estimator  does  not  track 
the  maneuvering  target  as  well  as  the  constant  velocity  model. 
Figures  61  through  63  are  the  Kalman  Filter  Gains  generated 
and  Figures  64  through  66  are  the  missile's  response  to  the 
uplink  commands.  The  uplinks  were  transmitted  using  the  same 
time  interval  as  in  the  constant  velocity  scenario.  A  miss 
distance  of  8.6  feet  occurred. 
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Figure  55.  Target  Position  in  the  X  Direction 
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Figure  56.  Target  Velocity  in  the  X  Direction 
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Figure  57.  Target  Position  in  the  Y  Direction 
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Figure  58.  Target  Velocity  in  the  Y  Direction 
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Figure  59.  Target  Position  in  the  Z  Direction 
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Figure  60.  Target  Velocity  in  the  Z  Direction 
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Figure  61.  Kalman  Gains  in  the  X  Direction 


KALMAN  CAINS  alhiY  DIRECTION 

•••  trosmoN  cain) 

♦  ♦♦    (VELOCITY  CAIN) 

■ 

■ 

▼                        '     * 

6 

--♦■                           • 

J                     •                                          |- 

....;. 

f 

-  ♦ * 

'■■                                          | 

I........... 

r 

♦ 

.  ....t 

♦ 
* 

i 

)                t 

2                      3                     4                     3 

* 

7 

1 

1 

TIME 

Figure  62.  Kalman  Gains  in  the  Y  Direction 
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Figure  63.  Kalman  Gains  in  the  Z  Direction 


Figure  64.  Seeker  Head  Angle  Rate  in  the  Yaw  Plane 
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Figure  65.  Seeker  Head  Angle  Rate  in  the  Pitch  Plane 
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Figure  66.  Commanded  Acceleration 
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The  computer  program  used  to  generate  the  plots  in  this 
chapter  is  included  in  the  appendix. 
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VI.   CONCLUSIONS  AND  RECOMMENDATIONS 

A.  CONCLUSIONS 

The  use  of  a  three-dimensional  model  in  simulation  studies 
is  beneficial  in  that  the  trajectories  of  the  missile  and 
target  can  be  displayed  in  all  three  axes,  therefore,  a  better 
understanding  of  the  behavior  of  the  systems  involved. 

The  simulation  studies  in  Chapter  III  demonstrated  that 
the  Proportional  Navigation  guidance  law  effectively  achieves 
intercept  of  the  target  for  both  the  maneuvering  and  non- 
manuvering  target.  Chapter  IV  demonstrated  the  need  for  the 
observer  concept  to  be  utilized  in  missile  design  to  reduce 
the  effects  of  noise.  Chapter  V  showed  an  additional  means  of 
countering  jamming  or  ECM  by  employing  the  ground  target 
observer  concept.  This  demonstrated  that  the  uplinks  in  target 
position  and  velocity,  for  the  constant  velocity  model,  were 
beneficial  in  a  noisy  environment. 

B .  RECOMMENDATIONS 

Further  studies  could  include  a  Kalman  Filter  in  the 
missile  design  instead  of  the  Luenberger  Observer.  It  is 
recommended  that  future  studies  include  the  use  of  a  two  part 
Kalman  Filter  with  the  ground  target  observer.  One  that 
estimates  the  states  and  the  other  being  a  maneuver  detector. 
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The  results  could  be  compared  with  the  present  filter  along 
with  ECM  effects  to  determine  the  effectiveness  of  the  two. 
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APPENDIX 


A.   MISSILE-TARGET  ENGAGEMENT  PROGRAM 


% 
% 
% 
% 


% 

% 
% 

% 

% 

% 

% 

% 

% 

% 

Am=[0 
0 
0 
0 
0 
0 


Bm=[ 


% 
% 
% 

% 

% 

% 

% 

% 

% 

Asp= 

Asy= 

Bsp= 


This  program  simulates  a  3  dimensional  target/missile 
engagement  using  basic  proportional  navigation  with 
an  asymptotic  observer. 

DEFINE  STATES 


Missile 

ms=[xm 
xdm 
ym 
ydm 
zm 
zdm 


missile  position  in  x 
missile  velocity  in  x 
missile  position  in  y 
missile  velocity  in  y 
missile  position  in  z 
missile  velocity  in  z 


direction 
direction 
direction 
direction 
direction 
direction] 


0 
0 
0 
0 

1 
0] 


0 
0 
0 
0 
0 

i]; 

Seeker  Head 

betap=[beta  pitch 
betad  pitch 

betay*[beta  yaw 
betad  yaw 


[0  1;-100  -20.0]  ; 
[0  1;-100  -20.0] ; 
[0;100]; 


-  seeker  head  pitch  angle 

■  seeker  head  pitch  angle  rate 

-  seeker  head  yaw  angle 

■  seeker  head  yaw  angle  rate] 
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Bsy=[0;100]; 
% 
% 
% 
% 
% 
% 
% 
% 
Ap=[-3  0;0  -3]; 


Autopilot 

gammamp= [ gammam  pitch  -  pitch  body  angle 

gammadm  pitch  -  pitch  body  angle  rate 

gammamy=[ gammam  yaw  -  yaw  body  angle 

gammadm  yaw  -  yaw  body  angle  rate] 


Bp=[3  0;0  3]; 


% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

At=[0 
0 
0 
0 
0 
0 


Target 

ts=[xt 
xdt 

yt 

ydt 

zt 

zdt 


0 
0 
0 
0 

1 

o]; 


target  position  in  x 
target  velocity  in  x 
target  position  in  y 
target  velocity  in  y 
target  position  in  z 
target  velocity  in  z 


direction 
direction 
direction 
direction 
direction 
direction] 


Bt=[0  0  0 
10  0 
0  0  0 
0  10 
0  0  0 

QOl]; 


% 

% 
% 

dt=.05; 


DISCRETE  REPRESENTATION 


[phisp,delsp]=  c2d(Asp, Bsp,dt) ; 
[phisy,delsy]=  c2d(Asy, Bsy,dt) ; 
[phim,delm]=  c2d(Am, Bm,dt) ; 
[phia,dela]=  c2d(Ap, Bp,dt) ; 
[phit,delt]=  c2d(At,Bt,dt) ; 

tfinal=  20.0; 
kmax=tf inal/dt  +1; 

%        INITIAL  VALUES 
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% 

%       navigation  ratio 

% 

NR= [4.0  0 ; 0  4.0]; 


%       initial  seeker  head  angles  and  angle  rates 

% 

beta_pitchO  =0.0; 

betad_pitchO=0 . 0 ; 

beta_yaw0   =0.0; 

betad_yawO  =0.0; 

betap ( :  ,  1 )  =  [ beta_pitchO 

betad_pitchO  ] ; 
betay ( : , 1 ) = [ beta_yaw0 

betad_yawO   ] ; 

%       initial  observer  angles  and  angle  rates 

betae_pitchO=0 . 0 ; 

betade_pitchO=0 . 0 ; 

betae_yawO=0 . 0 ; 

betaed_yawO=0 . 0 ; 

% 

% 

betaep ( : , 1 ) = [ betae_pitchO 

betade_pitchO] ; 
betaey ( : ,  1 )  =  [ betae_yawO 

betaed_yawO ] ; 
% 
% 

%       initial  missile  body  angle  rates 
% 

gammadm_pitchO=0 . 0 ; 
gammadm_yawO  =0.0; 

gammadm ( : , 1 ) = [ gammadm_pitchO 

gammadm_yawO   ] ; 
% 

%       initial  states  for  missile  flight  profile 
% 

xmO  =000.0; 
xdmO-2000.00; 
ymO  =000.0; 
ydm0=00; 
zmO  =00; 
zdm0=00; 
ms(:, l)=[xm0 

xdmO 

ymO 
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ydmO 
zmO 
zdmO] ; 

% 

%       initial   states  for  target  flight  profile 

% 

xtO  =20000; 

xdtO=0000; 

ytO  =050.0; 

ydt0=0300.0; 

ztO  =0051.0; 

zdtO=000.0; 

ts(: ,l)=[xtO 

xdtO 

ytO 

ydtO 

ztO 

zdtO] ; 
% 

%       initial  range  information 
% 

rxO=(xtO-xmO) ; 
rx(l)=rxO; 
ryO=(ytO-ymO) ; 
ry(l)=ryO; 
rzO=(ztO-zmO) ; 
rz (l)=rzO; 

rmO=sqrt(xmOA2  +  ymOA2  +  zm0*2) ; 
rm  ( 1 ) =rmO ; 

rtO=sqrt(xtO*2  +  ytOA2  +  zt0"2) ; 
rt(l)=rtO; 

rO  =sqrt( (xtO-xmO) A2  +  (ytO-ymO) A2  +  (ztO-zmO) A2) ; 
r(l)=rO; 


dist=[0.0013;0.0513] ; 

rand ( ' normal ' ) ; 

% 

%       initial  time 

% 

time(l)*0.0; 

% 

%        SIMULATION 

% 

for  (i=l:kmax-l) 

%      missile  and  target  flight  path  angles 

gammam_pitch(i)=atan2(ms(6/i)  , sqrt(ms(2,i)  A2+ms(4,i)  A2) ) ; 
gammam_yaw(i)=atan2 (ms(4, i) ,ms(2, i) ) ; 
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gammat_pitch(i)=atan2  (ts(6,i)  , sqrt(ts(2, i)  A2+ts(4,i)  A2) )  ; 
gammat_yaw(i)=atan2 (ts(4, i) ,ts(2,  i) )  ; 

%      target  velocity  (magnitude) 

vt  (i)=sqrt(ts(2/i) A2+ts(4,i) ~2+ts(6,i) A2) ; 

vm  (i)=sqrt(ms(2,  i)  A2+ms(4,  i)  A2+ms(6,  i)  A2)  ; 
% 

%       line-of-sight  angles 
% 

sigma_pitch(i)=atan2( (ts(5,i) -ms(5, i) ) ,sqrt( (ts(l, i) -ms(l, i) 
) A2  .  .  . 

+  (ts(3,i)-ms(3,i)r2)); 


sigma_yaw(i)=atan2 ( (ts(3, i)-ms(3, i) ) , ( (ts(l, i) -ms(l, i) ) ) ) ; 

sigma( : , i)=[sigma_pitch(i) 

sigma_yaw(i)   ] ; 


%       update  seeker  head 

% 

wk(i)=rand*.9; 


betap(:,i  +  l)=phisp*betap(:,i)    + 
delsp*sigma_pitch(i)+dist*wk(i)  ; 

betay( : , i+l)=phisy*betay ( : , i)  +  delsy*sigma_yaw(i) +dist*wk(i)  ; 

% 

%       set  up  observer  estimates 

% 

yO(l)-0.0; 

yl(l)=0.0; 

% 

% 

gain*[-0. 003l;0.0299] ; 

c-[0  1]; 

f=phisp-gain*c; 

yO(i+l)»c*betap(: ,  i) ; 

betaep( : , i+l)=f *betaep( : , i)    +    gain*y0(i)    + 
delsp*sigma_pitch(i)  ; 

yl(i+l)=c*betay(: ,  i)  ; 
f l=phisy-gain*c; 

betaey(: ,i+l)=fl*betaey(: ,i)    +    gain*yl(i)    + 
delsy*sigma_yaw(i) ; 
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% 
% 

%        seeker  head  angle  rate  vector 

% 

betad ( : ,  i )  =  [ betaep ( 2 , i ) 

betaey (2, i) ]  ; 

% 

%       update  autopilot 

%       acceleration  commanded  =  vm(i)  *  gammadm(i) 

%       gammadm(i)  =  Nav  constant  *  line  of  sight  rate 

gammadm(: ,  i+l)=phia*gammadm( : ,  i)  +  dela*NR*betad( : ,  i)  ; 

vm_pitch(i)=vm(i) *cos(gammam  yaw(i) -sigma_yaw(i) ) ; 
vm_yaw ( i ) =vm ( i ) *cos(gammam_pTtch(i) ) ; 

%       limit  commanded  acceleration  to  approximately  20  g's 
% 

if  (vm_yaw(i) *gammadm(2 , i) ) <=640. 0 

amc_y ( i ) = ( vm_yaw ( i ) *gammadm (2,1)); 
else 

amc_y(i) =640.0; 
end 
% 
% 

if (vm_pitch(i) *gammadm(l/ i) )<=640. 0 

amc_p(i)=(vm_pitch(i) *gammadm(l, i) ) ; 
else 

amc_p(i)=640.  0; 
end 
% 
%        magnitude  of  commanded  acceleration 

acom(i)=sqrt(amc_y(i) A2  +  amc_p(i)A2); 

%       missile  acceleration  vector  components 

xddm(i)»(gammadm(l, i) *ms(6, i) *cos(gammam_yaw(i) ) ) ; 
yddm ( i ) =amc_y ( i ) *cos ( gammam_yaw ( i ) ) ; 
zddm(i)=amc_p(i) *cos(gammam_pitch(i) ) ; 

%        total  missile  acceleration  magnitude 
% 

am(i)=sqrt(xddm(i) "2  +  zddm(i)~2  +  yddm(i)*2  ); 
% 
%        missile  input  vector 


78 


um= [ xddm ( i ) 
yddm ( i ) 
zddm(i) ] ; 
% 

%       update  missile 
% 

ms( : , i+l)=phim*ms( : , i)  +  delm*um; 

% 

%       set  target  acceleration  components 

% 

if  (r(i)<=21000.0) 

xddt(i)=-5.0*3  2.2*sin(sigma_yaw(i) ) ; 
yddt(i)=  3 . 5*32.2*cos(sigma_yaw(i) ) ; 
zddt(i)=  3.5*32.2*cos(sigma_pitch(i) ) ; 
else 

xddt(i)=0.0; 
yddt(i)=0.0; 
zddt(i)=0.0; 
end 

% 

%       target  acceleration  magnitude 

% 

at(i)=sqrt(xddt(i) A2  +  yddt(i)A2  +  zddt(i)A2); 
% 

%        target  input  vector 
% 

ut=[xddt(i) 
yddt(i) 
zddt(i)]; 
% 

%       update  target 
% 

ts(: , i+l)=phit*ts(: ,i)  +  delt*ut; 
% 
% 

%       missile  and  target  trajectory  data 
% 

missile(i, : )»[ms(l, i)  ms(3,i)  ms(5,i)]; 

target(i,:)  =[ts(l,i)  ts(3,i)  ts(5,i)]; 


%  update  range  information 
% 

r(i+l)-sqrt((ts(l,i+l)-ms(l,i+l))A2    + 
(ts(3,i+l)-ms(3,i+l) )  A2. . . 

+  (ts(5,i+l)-ms(5,i+l)) A2) ; 
rm(i+l)=sqrt(ms(l,i+l) A2  +ms(3,i+l)  A2  +  ms (5,1+1)  A2)  ; 
rt(i+l)=sqrt(ts(l,i+l) A2  + ts(3,i+l) A2  + ts(5,i+l) A2) ; 
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%       update  time  vector 

% 

time(i+l)=time(i)  +  dt; 

% 

%        check  to  see  if  engagement  is  at  closest  point  of 

approach 

% 

if (r (i)<r(i+l) ) , break, end 
end; 
% 

%        PLOT  RESULTS 
% 

%       range  data 
% 

plot(time,r) , grid, xlabel( 'TIME' ) ,ylabel( 'FEET' ) 
title ('RANGE  VS  TIME') 

text(  7,20000, ['rng=' , num2str (r (i) ), '  feet']); 
meta  msltgt, pause, clg 

subplot(211)  ,  plot  (time,  rm)  ,  grid,  xlabel(  'TIME' )  ,  y  label  ( 'FEET' ) 

title ('MISSILE  RANGE  VS  TIME'); 

subplot(212)  ,plot(time,rt)  ,  grid,  xlabel( 'TIME' )  ,  y  label  ( 'FEET' ) 

title('TARGET  RANGE  VS  TIME'); 

meta  jerryl, pause, clg 

% 

% 

plot (ms ( 1 , : ) , ms ( 3 , : ) , ts ( 1 , : ) , ts ( 3 , : ) ) , grid 

title ('MISSILE-TARGET  TRAJECTORY  IN  THE  X-Y  PLANE') 

xlabel('X  AXIS  IN  FEET' ) ,ylabel ( ' Y  AXIS  IN  FEET') 

gtext( 'MISSILE') 

gtext(' TARGET') 

meta  msltgt, pause, clg 

plot(ms(l, : ) ,ms(5, : ) , ts(l, : ) ,ts(5, : ) ) ,grid 

title ('MISSILE-TARGET  TRAJECTORY  IN  THE  X-Z  PLANE') 

xlabel('X  AXIS  IN  FEET' ) ,ylabel ( ' Z  AXIS  IN  FEET') 

gtext( 'MISSILE') 

gtext ( ' TARGET ' ) 

meta  msltgt, pause, clg 

%       missile  and  target  velocity 

% 

plot (time (l:i) ,vm) ,grid,xlabel( 'TIME' ) ,ylabel( 'FEET/SEC ) 

title ('MISSILE  VELOCITY  VS  TIME'); 

meta  jerryl, pause, clg 

plot(time(l:i)  ,vt)  ,grid,xlabel('TIME')  ,ylabel( 'FEET/SEC  ) 
title( 'TARGET  VELOCITY  VS  TIME'); 
meta  jerryl, pause, clg 

%      missile  and  target  acceleration 
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plot (time (1: i) ,  am) , grid, xlabel( 'time' ) ,  y label ( 'feet/ see* 2' ) 

title( 'Missile  acceleration  vs  Time') 

meta  jerryl, pause ,clg 

plot (time (1: i) ,at) , grid,xlabel ( 'time' ) , y label ( 'feet/secA2' ) 

title ( 'Target  acceleration  vs  Time') 

meta  jerryl, pause ,  clg 

%      seeker  head  angle  rates 

plot(time(l:i-l) ,betay(2, (1:1-1) ) ) ,grid,xlabel( 'time' ) ,ylabe 

1( 'radians' ) 

title ( 'Seeker  head  Yaw  angle  rate  vs  Time') 

meta  jerryl, pause, clg 

plot(time(l:i-l) ,betap(2, (1:1-1) ) ) , grid, xlabel( 'time' ) ,ylabe 

1( 'radians' ) 

title ( 'Seeker  head  Pitch  angle  rate  vs  Time') 

meta  jerryl, pause, clg 

%      commanded  acceleration 

plot(time(l: i) ,acom) ,grid, title ( 'acceleration  commanded') 
meta  jerryl, pause, clg 

END 
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B.   MISSILE-TARGET  PROGRAM  WITH  TARGET  OBSERVER 


clear 

! erase  msltgt.met 

! erase  jerryl.met 

clg 

% 

% 

% 

% 

% 

% 


% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

% 

Am=[0 
0 
0 
0 
0 
0 


Bm=[ 


% 
% 
% 

% 

% 

% 

% 

% 

% 

Asp= 

Asy= 

Bsp= 

Bsy= 


This  program  simulates  a  3  dimensional  target/missile 
engagement  using  basic  proportional  navigation  with 
an  Luenberger  observer  and  a  ground  target  observer. 

DEFINE  STATES 


Missile 

ms=[xm 
xdm 
ym 
ydm 
zm 
zdm 


missile  position  in  x 
missile  velocity  in  x 
missile  position  in  y 
missile  velocity  in  y 
missile  position  in  z 
missile  velocity  in  z 


direction 
direction 
direction 
direction 
direction 
direction] 


0 
0 
0 
0 

1 
0] 


0 
0 
0 
0 
0 

i]; 

Seeker  Head 

betap=[beta  pitch 
betad  pitch 

betay»[beta  yaw 
betad  yaw 


[0  1;-100  -20.0] ; 
[0  1;-100  -20.0]; 
[0;100]; 
[0;100]; 


-  seeker  head  pitch  angle 

■  seeker  head  pitch  angle  rate 

-  seeker  head  yaw  angle 

-  seeker  head  yaw  angle  rate] 
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% 
% 

% 
% 

% 
% 
% 
% 


Autopilot 

gammamp= [ gammam  pitch  -  pitch  body  angle 

gammadm  pitch  -  pitch  body  angle  rate   ] 

gammamy= [ gammam  yaw  -  yaw  body  angle 

gammadm  yaw  -  yaw  body  angle  rate] 


Ap=[-3  0;0  -3]; 


Bp=[3  0;0  3]; 


% 
% 
% 

% 

% 

% 

% 

% 

% 

At=[0 
0 
0 
0 
0 
0 

Bt=[0 
1 
0 
0 
0 
0 


% 
% 

% 

dt=.05; 


Target 

ts=[xt 
xdt 

yt 

ydt 

zt 

zdt 


target  position  in 
target  velocity  in 
target  position  in 
target  velocity  in 
target  position  in 
target  velocity  in 


x  direction 
x  direction 
y  direction 
y  direction 
z  direction 
z  direction] 


0 
0 
0 
0 

1 
0] 


0 
0 
0 
0 
0 

IN- 
DISCRETE REPRESENTATION 


[phisp,delsp]=  c2d(Asp,Bsp,dt) ; 
[phisy,delsy]=  c2d(Asy,Bsy,dt)  ; 

[phim,delm]=  c2d (Am, Bm,dt) ; 
[phia,dela]=  c2d(Ap,Bp,dt) ; 
[phit,delt]=  c2d(At,Bt,dt) ; 

tfinal=  20.0; 
kmax=tfinal/dt  +1; 


INITIAL  VALUES 
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%       navigation  ratio 

% 

NR= [4.0  0 ; 0  4.0]; 


%       initial  seeker  head  angles  and  angle  rates 

% 

beta_pitchO  =0.0; 

betad_pitch0=0 . 0 ; 

beta_yaw0   =0.0; 

betad_yawO  =0.0; 

betap ( :  ,  1 )  =  [ beta_pitchO 

betad_pitchO  ] ; 
betay ( : ,  1 )  =  [ beta_yaw0 

betad_yawO   ] ; 

%       initial  line-of-sight  angular  rates 

s  igmad_pitchO=0 . 0 ; 
s  iguiad_yawO     =0.0; 

s  igmad ( : , 1 )  =  [ s  igmad_pitchO 
sigmad_yawO  ] ; 

%       initial  observer  angles  and  angle  rates 

betae_pitchO=0 . 0 ; 

betade_pitchO=0 . 0 ; 

betae_yawO=0 . 0 ; 

betaed_yawO=0 . 0 ; 

% 

% 

betaep ( : , 1 ) = [ betae_pitchO 

betade_pitchO] ; 
betaey ( : , 1 ) * [ betae_yawO 

betaed_yawO] ; 
% 
% 

%       initial  missile  body  angle  rates 
% 

gammadm_pitch0=0 . 0 ; 
gammadm_yaw0  =0.0; 

gammadm( : , l)=[gammadmjpitch0 

gammadm  yawO   ] ; 
% 

%       initial  missile  states 
% 

xmO  =000.0; 
xdm0=2000.00; 
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ymO  =000.0; 
ydmO=00; 
zmO  =00; 
zdmO=00; 
ms( :  , l)=[xmO 

xdmO 

ymO 

ydmO 

zmO 

zdmO] ; 
% 

%       initial  target  states 
% 

xtO  =20000; 
xdtO=0000; 
ytO  =050.0; 
ydtO=1000.0; 
ztO  =050.0; 
zdt0=000.0; 

ts(: ,l)=[xtO 

xdtO 

ytO 

ydtO 

ztO 

zdtO] ; 
% 

%       initial  range  information 
% 

rxO=(xtO-xmO) ; 
rx(l)=rxO; 
ryO=(ytO-ymO) ; 
ry(l)=ryO; 
rzO=(ztO-zmO) ; 
rz (l)=rzO; 

rm0=sqrt(xm0*2  +  ym0*2  +  zmO*2) ; 
rm ( 1 ) =rmO ; 

rt0=sqrt(xt0*2  +  ytOA2  +  zt0*2) ; 
rt(l)=rtO; 

rO  =sqrt( (xt0-xm0)*2  +  (yt0-ym0)*2  +  (ztO-zmO) *2) ; 
r(l)=rO; 
%       Initial  Target  Tracker  values 

y(:,l)=[0;0;0]; 

xhatl    =[0;0;0;0;0;0] ; 

pkkml=[10*9  0  0  0  0  0 
0  10*9  0  0  0  0 
0  0  10*9  0  0  0 
0  0  0  10*9  0  0 
0  0  0  0  10*9  0 
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0    0    0    0    0    10*9] ; 

rand( 'normal' ) ; 

h=[l  0  0  0  0  0 
0  0  10  0  0 
0    0    0    0    10]; 

aT=[0  10  0  0  0 
0  0  0  0  0  0 
0  0  0  10  0 
0  0  0  0  0  0 
0  0  0  0  0  1 
0    0    0    0    0    0]; 

bT=[0  0  0 
10  0 
0  0  0 
0  10 
0  0  0 
0    0    1]; 

dist=[0.0013;0.0513] ; 


%  Continuous  to  Discrete 
% 

[phi,del]=c2d(aT,bT,dt) ; 
% 

%       initial  time 
% 

time(l) =0.0; 
% 
%       SIMULATION 

j=o; 

for  (i=l:kmax-l) 

j-j+i; 

%      missile  and  target  flight  path  angles 


gammamjpitch(i)=atan2  (ms(6,i)  , sqrt(ms(2,  i)  *2+ms(4,i)  A2) )  ; 
gammam_yaw(i)*atan2 (ms(4/ i) ,ms(2, i) ) ; 

gammat_pitch(i)=atan2  (ts(6,i)  ,sqrt(ts(2,i)  *2+ts(4,i)  A2) )  ; 
gammat_yaw(i)»atan2(ts(4,i) , ts(2, i) ) ; 


%      target  velocity  (magnitude) 

vt  (i)=sqrt(ts(2,i) *2+ts (4 , i) A2+ts(6, i) A2) ; 

vm  (i)=sqrt(ms(2,i)  A2+ms(4/i)  *2+ms(6,i)  A2)  ; 
% 
%       line-of-sight  angles 
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sigma_pitch(i)=atan2 ( (ts(5, i) -ms(5, i) ) ,sqrt( (ts(l, i) -ms(l, i) 
)  "2 . 

+  (ts(3,i)-xns(3,i))*2))  ; 


sigma  yaw(i) =atan2 ( (ts (3 , i) -ms(3 ,  i) ) , ( (ts(l, i) -ms(l, i) ) ) ) ; 


sigma ( : ,  i) =[sigma_pitch(i) 

sigma_yaw(i)   ] ; 


%       update  seeker  head 

wk(i)=rand*.9; 
% 

betap(    :    ,    i  +  1  )   =phisp*betap(    :    ,    i) 
delsp*sigma_pitch ( i) +dist*wk (i) ; 

betay(    :    ,    i  +  1  )   =phisy*betay(    :    ,    i) 
delsy*sigma_yaw(i) +dist*wk(i)  ; 

% 

%       set  up  observer  estimates 
% 

yO(l)=0.0; 

yl(l)=0.0; 

% 

% 

gain=[-0.0031;0.0299] ; 

c=[0  1]; 

f =phisp-gain*c ; 

yO(i+l)=c*betap( : ,  i) ; 

betaep( : , i+l)=f *betaep( : , i)    +    gain*yO(i) 
delsp*sigma_pitch(i) ; 

yl(i+l)=c*betay(: ,i) ; 

f l=phisy-gain*c ; 

betaey( : ,i+l)=fl*betaey( : ,i)    +    gain*yl(i) 
delsy*sigma__yaw(i)  ; 
% 
% 
% 

%        seeker  head  angle  rate  vector 
%       with  uplinks  at  one  second  intervals 


if  (j— 40) 

betad( :,!)=[ (betaep(2, i) +sigmad(l, i) ) 

(betaey(2, i)+sigmad(2f  i) ) ] ; 
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j=o; 
else 

betad ( : , i ) = [ betaep ( 2 , i ) 

betaey (2, i) ] ; 
end 


%       update  autopilot 

%       acceleration  commanded  =  vm(i)  *  gammadm(i) 

%       gammadm(i)  =  Nav  constant  *  line  of  sight  rate 

gammadm( : ,  i+l)=phia*gammadm( : ,  i)  +  dela*NR*betad( : ,  i)  ; 

vm_pitch(i)=vm(i) *cos(gammam  yaw(i) -sigma_yaw(i) ) ; 
vm_yaw ( i ) =vm ( i ) *cos(gammam_pitch(i) ) ; 

%       limit  commanded  acceleration  to  approximately  20  g's 
% 

if  (vm_yaw(i) *gammadm(2, i) ) <=640.0 

amc_y ( i ) = ( vm_yaw ( i ) *gammadm ( 2 ,  i ) ) ; 
else 

amc_y(i) =640.0; 
end 
% 
% 

if (vm_pitch(i) *gammadm(l, i) )<=640. 0 

amc_p(i)=(vm_pitch(i) *gammadm(l, i) ) ; 
else 

amc_p(i) =640.0; 
end 
% 
%       magnitude  of  commanded  acceleration 

acom(i)=sqrt(amc_y(i) A2  +  amc_p(i)"2); 

%        missile  acceleration  vector  components 

xddm(i)=(gammadm(l, i) *ms(6, i) *cos(gammam_yaw(i) ) ) ; 
yddm  ( i )  »amc__y  ( i )  *cos  ( gammam_yaw  ( i ) )  ; 
zddm(i)=amc_p(i) *cos(gammam_pitch(i) ) ; 

%        total  missile  acceleration  magnitude 
% 

am(i)=sqrt(xddm(i) A2  +  zddm(i)A2  +  yddm(i)A2  ); 
% 

%       missile  input  vector 
% 
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um=[xddm(i) 
yddm ( i ) 
zddm(i) ] ; 
% 

%       update  missile 
% 

ms( : , i+l)=phim*ms( : , i)  +  delm*um; 
% 

%       set  target  acceleration  components 
% 

if  (r(i)<=00000.0) 

xddt (i) ■  -5 . 0*32 . 2*sin(sigma_yaw(i) ) ; 
yddt(i)=  3 . 5*32 . 2*cos (sigma_yaw(i) ) ; 
zddt(i)=  3 . 5*32.2*cos(sigma_pitch(i) ) ; 
else 

xddt (i) =0.0 ; 
yddt(i)=0.0; 
zddt(i)=0.0; 
end 

%       target  acceleration  magnitude 
% 

at(i)=sqrt(xddt(i)  A2  +  yddt(i)*2  +  zddt(ip2)  ; 
% 

%        target  input  vector 
% 

ut=[xddt(i) 
yddt(i) 
zddt(i) ]; 
% 

%       update  target 
% 

ts(: ,i+l)=phit*ts(: ,i)  +  delt*ut; 
% 
% 

%       missile  and  target  trajectory  data 
% 

missile(i, : J^tmsfl, i)  ms(3,i)  ms(5,i)]; 

target(i,:)  =[ts(l,i)  ts(3,i)  ta(5,i))j 
% 
%      Target  traker 

%      Input  initial  conditions 
% 

x(:,i)-[ts(l,i);ts(2,i);ts(3,i);ts(4,i);ts(5,i);ts(6,i)]; 
% 

%   Plant  noise  covariance 
% 

Q1=[160A2  0  0;0  160A2  0;0  0  160*2]; 
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Measurement  noise  covariance 


R=[500A2  0  0 
0  125A2  0 
0  0  12 A2]  ; 


%   Input  equations 


Q=[160A2;160A2;160*2] ; 

nu=[sqrt(500) A2 ;sqrt (125*2) ;sqrt(12A2) ]*rand; 

w(:,i)  =sqrt(Q) *rand; 


%  Update  the  plant  states 
% 

x( : , i+l)=phi*x( : , i)   +del*w( : , i) ; 

y( : , i+l)=h*x( : , i+1)   +nu; 
% 

%  Compute  Kalman  gains 
% 

G  =  pkkml*h/*inv(h*pkkml*h/+R) ; 
% 
%  Define  gain  matrix  for  plotting 

gO,!)  =[G(1 :  2,1)  ;G  (3:4,2)  ;G(5:  6,3)]; 
% 
%  Measurement  step 

xhat  (:,i+l)  =xhatl+(G  * (y( : , i+1) -h*xhatl) ) ; 

pkk         =(eye(6)-G  *h)*pkkml; 
% 
%  Movement  step 

xhatl=phi*xhat    ( : , i+1) ; 
pkJcml=phi*pkk*phi'+  del*Ql*del'; 

%  compute  the  range  deviation 

xhatr  =[xhat(l, i) ;xhat (3 , i) ;xhat(5, i) ] ; 

yr    =y(:,i); 

xr(:,i)»   (yr  -  xhatr); 

%  compute  the  velocity  deviation 

xhatv  =[xhat(2, i) ;xhat(4,i) ;xhat(6,i) ] ; 
yv     =[ts(2,i);ts(4,i);ts(6,i)]; 
xv(:,i)=    (yv  -  xhatv); 
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%  Transformation  to  body  axes 

all=cos(gammam_pitch(i) ) *cos (gammam_yaw(i) ) ; 
al2=cos(gammam_pitch(i) ) *sin(gammam_yaw(i) ) ; 
al3=(-sin(gammam_pitch(i) ) ) ; 
a21=(-sin(gammam_yaw(i) ) ) ; 
a22=cos(gammam_yaw( i) ) ; 
a2  3=0.0; 

a31=sin(gammam_pitch(i) ) *cos(gammam_yaw(i) ) ; 
a32=sin(gammam_pitch(i) ) *sin(gammam_yaw(i) ) ; 
a3  3=cos(gammam_pitch(i) )  ; 

%  velocity  transformation 

vbx(i)=xv(l/i)*all+xv(2,i)*al2+xv(3/i) *al3; 
vby(i)=xv(l,i)*a21+xv(2/i)*a22+xv(3,i)*a23; 
vbz(i)=xv(l,i)*a31+xv(2/i)*a32+xv(3/i)*a33; 

%  range  transformation 

rbx(i)=xr(l/i)*all+xr(2,i)*al2+xr(3/i)*al3; 
rby(i)=xr(l,i)*a21+xr(2,i)*a22+xr(3,i)*a2  3; 
rbz(i)=xr(l,i)*a31+xr(2/i)*a32+xr(3,i)*a3  3; 

%   line-of-sight  transformation 

sll=cos(sigma_pitch(i) ) *cos(sigma_yaw(i) ) ; 
sl2=cos(sigma_pitch(i) ) *sin(sigma_yaw(i) ) ; 
sl3=(-sin(sigma_pitch(i) ) ) ; 
s21=(-sin(sigma_yaw(i) ) ) ; 
s22=cos(sigma_yaw(i) ) ; 
s2  3=0.0; 

s31=sin(sigma_pitch(i) ) *cos(sigma_yaw(i) ) ; 
s32=sin(sigma_pitch(i) ) *sin(sigma_yaw(i) ) ; 
s33=cos(sigma_pitch(i) ) ; 

%  velocity  transformation 

vsx(i)=vbx(i) *sll+vby(i) *sl2+vbz (i) *sl3; 
vsy(i)=vbx(i) *s21+vby(i) *s22+vbz(i) *s23; 
vsz (i)=vbx(i) *s31+vby (i) *s32+vbz (i) *s33 ; 

rsx(i)=rbx(i)*sll+rby(i)*sl2+rbz(i) *sl3; 
rsy(i)=rbx(i) *s21+rby(i) *s22+rbz(i) *s23; 
rsz(i)=rbx(i) *s31+rby(i) *s32+rbz(i) *s33; 

%  compute  the  magnitude  of  the  range  deviation 

rd(i)=sqrt(rsx(i) "2+rsy(i) A2+rsz(i) A2) ; 

%  generate  the  angular  rate  deviations 
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sigmad_pitch(i+l)=  vsy(i)/ (r(i)+rd(i) ) ; 
sigmad_yaw(i+l)=  vsz (i) / (r (i) +rd(i) ) ; 

sigmad( : , i+l)=[sigmad_pitch(i) 

sigmad_yaw(i) ] ; 


%  update  range  information 
% 

r(i+l)=sqrt(  (ts(l,i  +  l)-ms(l,i  +  l)  )  A  2    + 
(ts(3,i+l)-ms(3,i+l))  A2. . . 

+  (ts(5,i+l)-ms(5,i+l)) A2) ; 
rm(i+l)=sqrt(ms(l,i+l)  "2  +  ms(3,i+l)  A2  +  ms(5,i+l)  "2)  ; 
rt(i+l)=sqrt(ts(l,i+l) A2 + ts(3/i+l) A2  +  ts(5,i+l) A2) ; 

%       update  time  vector 
% 

time(i+l)=time(i)  +  dt; 
% 

%        check  to  see  if  engagement  is  at  closest  point  of 
approach 
% 

if (r(i)<r(i+l) ) , break, end 
end; 
% 

%        PLOT  RESULTS 
% 

%       range  data 
% 

plot (time, r) , grid, xlabel ( 'TIME' ) ,ylabel ( 'FEET' ) 
title ('RANGE  VS  TIME') 

text(  7,20000, ['rng=',num2str(r(i) ), '  feet']); 
meta  msltgt, pause, clg 

subplot  (211)  ,  plot  (time,  rm)  ,  grid,  xlabel  ( 'TIME' )  ,ylabel('FEET') 

title ('MISSILE  RANGE  VS  TIME'); 

subplot  (2 12)  ,  plot  (time,  rt)  ,  grid,  xlabel  ( 'TIME' )  ,  y label  ('  FEET' ) 

title( 'TARGET  RANGE  VS  TIME'); 

meta  msltgt, pause, clg 

% 

% 

%       missile  and  target  velocity 

% 

%plot(time(l:i) ,vm) ,grid, xlabel ( 'TIME') ,ylabel( 'FEET/SEC ) 

%title( 'MISSILE  VELOCITY  VS  TIME'); 

%meta  msltgt, pause, clg 

%plot(time(l:i) ,vt) ,grid,xlabel('TIME') ,ylabel( 'FEET/ SEC ) 
%title(' TARGET  VELOCITY  VS  TIME'); 
%meta  msltgt, pause, clg 
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%      missile  and  target  acceleration 

plot (time (l:i) ,am) ,grid,xlabel ( 'TIME' ) ,ylabel ( 'FEET/SEC* 2 ' ) 

title ('MISSILE  ACCELERATION  vs  TIME') 

meta  msltgt, pause, clg 

plot(time(l: i) ,  at) , grid, xlabel ( 'time' ) ,y label ( 'feet/secA2' ) 

title ('TARGET  ACCELERATION  vs  TIME') 

meta  msltgt , pause, clg 

%      seeker  head  angle  rates 

plot (time (1: i-1) ,betaey(2, (1: i-l) ) ) ,grid,xlabel ( 'time' ) ,ylab 

el( 'radians' ) 

title ('SEEKER  HEAD  YAW  ANGLE  RATE  vs  TIME') 

meta  msltgt, pause, clg 

plot (time(l: i-l) ,betaep(2, (1:1-1) ) ) ,grid,xlabel ( 'time' ) ,ylab 

el ( 'radians' ) 

title ('SEEKER  HEAD  PITCH  ANGLE  RATE  vs  TIME') 

meta  msltgt, pause, clg 

%      commanded  acceleration 

plot (time (l:i) ,acom) ,grid, title ( 'ACCELERATION  COMMANDED') 
xlabel ( 'TIME' ) ,ylabel ( 'FEET/SEC2 ' ) 
meta  msltgt, pause, clg 


%      Kalman  estimates 

plot (time (1: i-l) ,x(l, (l:i-l) ) , '*' ,time(l: i-l) ,xhat(l, (1:1-1) 

),'  +  ') 

xlabel ('TIME') ,ylabel ( 'FEET' ) ,grid 

title( 'TARGET  POSITION  in  the  X  DIRECTION  vs  TIME') 

gtext('**  ACTUAL  TARGET  STATE   ') 

gtext('++  KALMAN  FILTER  ESTIMATE') 

meta  msltgt, pause, clg 

plot (time (1: i-l) ,x(2, (1:1-1)) , '*' ,time(l: i-l) ,xhat(2, (l:i-l) 

),'  +  ') 

xlabel ('TIME') , y label ( 'FEET/ SEC ) ,grid 

title ('TARGET  VELOCITY  in  the  X  DIRECTION  vs  TIME') 

gtext('**  ACTUAL  TARGET  STATE    ') 

gtext('++  KALMAN  FILTER  ESTIMATE') 

meta  msltgt, pause, clg 

plot (time (l:i-50) ,g(l, (l:i-50) ) , '*' ,time(l: i-50) ,g(2, (l:i-50 

)),'+') 

title ('KALMAN  GAINS  in  the  X  DIRECTION' ), grid, xlabel ( 'TIME' ) , 
gtext('***   (POSITION  GAIN) ') 
gtext ( ' +++   (VELOCITY  GAIN) ' ) 
meta  msltgt, pause, clg 
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%      Kalman  estimates 

plot (time(l: i-l) ,x(3, (1:1-1) ) , '*' , time(l: i-l) ,xhat(3, (l:i-l) 

),'+') 

xlabel ('TIME')  ,ylabel ('FEET' )  ,grid 

title ('TARGET  POSITION  in  the  Y  DIRECTION  vs  TIME') 

gtext('**  ACTUAL  TARGET  STATE   ') 

gtext  C++  KALMAN  FILTER  ESTIMATE') 

meta  msltgt, pause, clg 

plot ( time (1: i-l) ,x(4, (l:i-l) ) , '*', time (1: i-l) /xhat(4/ (l:i-l) 

),'+') 

xlabel('TIME') , ylabel ( 'FEET/SEC ) ,grid 

title ('TARGET  VELOCITY  in  the  Y  DIRECTION  vs  TIME') 
gtext ('**  ACTUAL  TARGET  STATE    ') 
gtext ('++  KALMAN  FILTER  ESTIMATE') 
meta  msltgt, pause, clg 

plot(time(l:i-50) ,g(3, (1:1-50) ) , ' *' , time(l: i-50) ,g(4, (l:i-50 

)),'+') 

title ('KALMAN  GAINS  in  the  Y  DIRECTION' ), grid, xlabel ( 'TIME' ) , 

gtext ('***   (POSITION  GAIN) ') 

gtext ( ' +++   (VELOCITY  GAIN) ' ) 

meta  msltgt, pause, clg 

plot (time(l: i-l) ,x(5, (1:1-1) ) , '*' , time (1: i-l) ,xhat(5, (l:i-l) 

),'+') 

xlabel ('TIME') , ylabel ( 'FEET' ) ,grid 

title ('TARGET  POSITION  in  the  Z  DIRECTION  vs  TIME') 

gtext ('**  ACTUAL  TARGET  STATE   ') 

gtext ('++  KALMAN  FILTER  ESTIMATE') 

meta  msltgt, pause, clg 

plot (time (1: i-l) ,x(6, (1:1-1) ) , '*' , time (1: i-l) ,xhat(6, (1:1-1) 

),'+') 

xlabel ('TIME') , ylabel ( 'FEET/SEC ) ,grid 

title( 'TARGET  VELOCITY  in  the  Z  DIRECTION  vs  TIME') 

gtext ('**  ACTUAL  TARGET  STATE    ') 

gtext ('++  KALMAN  FILTER  ESTIMATE') 

meta  msltgt, pause, clg 

plot (time (1: i-50) ,g(5, (l:i-50) ) , '*' , time (1: i-50) ,g(6, (l:i-50 

)),'+') 

title ('KALMAN  GAINS  in  the  Z  DIRECTION' ), grid, xlabel ('TIME' ) , 

gtext ('***   (POSITION  GAIN) ') 

gtext ( ' +++   (VELOCITY  GAIN) ' ) 

meta  msltgtl, pause, clg 

end 
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